﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using MathLib;
using TrackingSim.GameSystem;
using TrackingSim.Filtering.Tracks;

namespace TrackingSim.Filtering.Measurements
{
    public class Razel : Measurement
    {
        double range;
        double az;
        double el;
        Vector unit_los;
        double range_random_std;
        double range_bias_std;
        double az_random_std;
        double az_bias_std;
        double el_random_std;
        double el_bias_std;
        Vector true_pos;
        double true_range;

        public Razel():base(0.0)
        {
            range = 0.0;
            az = 0.0;
            el = 0.0;
            range_random_std = 0.0;
            range_bias_std = 0.0;
            az_random_std = 0.0;
            az_bias_std = 0.0;
            el_random_std = 0.0;
            el_bias_std = 0.0;
            unit_los = new Vector(3);
            true_pos = new Vector(3);
            true_range = 0.0;
        }

        public Razel(double _time, double _range, double _az, double _el):base(_time)
        {
            range = _range;
            az = _az;
            el = _el;
            range_random_std = 0.0;
            range_bias_std = 0.0;
            az_random_std = 0.0;
            az_bias_std = 0.0;
            el_random_std = 0.0;
            el_bias_std = 0.0;
            unit_los = new Vector(3);
            true_pos = new Vector(3);
            true_range = 0.0;
        }

        public override Measurement clone()
        {
            Razel copy = new Razel();

            copy.Time = Time;
            copy.range = range;
            copy.az = az;
            copy.el = el;
            copy.range_random_std = range_random_std;
            copy.range_bias_std = range_bias_std;
            copy.az_random_std = az_random_std;
            copy.az_bias_std = az_bias_std;
            copy.el_random_std = el_random_std;
            copy.el_bias_std = el_bias_std;
            copy.unit_los = unit_los.clone();
            copy.true_pos = true_pos.clone();
            copy.true_range = true_range;

            return copy;
        }

        public override void accept(IVisitor visitor, Player p)
        {
            visitor.visit(this, p);
        }

        private static double sqr(double x)
        {
            return x * x;
        }

        public override Matrix get_r()
        {
            Matrix R = new Matrix(3, 3);

            R.m[0, 0] = sqr(R_random_std);
            R.m[1, 1] = sqr(Az_random_std);
            R.m[2, 2] = sqr(El_random_std);

            return R;
        }

        public override Matrix get_z()
        {
            MathLib.Matrix Z = new Matrix(3, 1);

            Z.m[0, 0] = R;
            Z.m[1, 0] = Az;
            Z.m[2, 0] = El;

            return Z;
        }

        public override Vector get_delta_body()
        {
            Vector delta_body = R * UnitLOS;
            return delta_body;
        }

        public double R
        {
            get { return range; }
            set { range = value; }
        }

        public double Az
        {
            get { return az; }
            set { az = value; }
        }

        public double El
        {
            get { return el; }
            set { el = value; }
        }

        public Vector UnitLOS
        {
            get { return unit_los; }
            set { unit_los = value; }
        }

        public double R_random_std
        {
            get { return range_random_std; }
            set { range_random_std = value; }
        }

        public double R_bias_std
        {
            get { return range_bias_std; }
            set { range_bias_std = value; }
        }

        public double Az_random_std
        {
            get { return az_random_std; }
            set { az_random_std = value; }
        }

        public double Az_bias_std
        {
            get { return az_bias_std; }
            set { az_bias_std = value; }
        }

        public double El_random_std
        {
            get { return el_random_std; }
            set { el_random_std = value; }
        }

        public double El_bias_std
        {
            get { return el_bias_std; }
            set { el_bias_std = value; }
        }

        public Vector TruePos
        {
            get { return true_pos; }
            set { true_pos = value; }
        }

        public double TrueR
        {
            get { return true_range; }
            set { true_range = value; }
        }
    }
}
